/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 * 
 * IMPORTANT : pour un tic on peut : 
 * A l'arrêt, les commandes simultanément possibles sont :

 * 1) démarrer (setMovingState(MOVING)), enclencher ou stopper le couteau (setCutterState(ON/OFF))
 * 2)  rester à l'arrêt, tourner (changeOrientation()), consulter le radar (updateRadarData()), 
 *   enclencher ou stopper le couteau (setCutterState(ON/OFF))

En mouvement, les commandes simultanément possibles sont :

 *  1) s'arrêter (setMovingState(STOPPED)), enclencher ou stopper le couteau (setCutterState(ON/OFF))
 *  2) rester en mouvement, enclencher ou stopper le couteau (setCutterState(ON/OFF))
 * 
 */
package kuti.grp5;

import java.awt.geom.Point2D;
import java.util.Iterator;
import java.util.Map;
import java.util.Random;
import kuti.*;

/**
 *
 * @author Pierre & Jonathan
 */
public class Driver extends Thread implements IDriver {

    private double minEnergy = 0;
    private static final int MAXROTATE = 80;
    private static final double MAXSCHIFT = Math.PI / 17;
    private KutiMap kutiMap;
    private IActuator ia;
    private IObservableKuti iok;
    private IRadar iradar;
    private ISimulParams isp;
    private boolean newSearch = true;
    private int id = 0;
    private boolean powerUnitExists = true;
    private boolean alined = false;
    private int cptTurn=0;
    private boolean isCutting = false;
    private boolean isMoving = false;

    public Driver() {
        kutiMap = new KutiMap();
    }

    @Override
    public void startDriverThread(IActuator ia, IObservableKuti iok, IRadar iradar, ISimulParams isp) {
        this.ia = ia;
        this.iok = iok;
        this.iradar = iradar;
        this.isp = isp;
        newSearch = true;
        currentThread().setName("Kuti: " + id++);
        minEnergy = calculatePowerLimit();
        //System.out.println(calculatePowerLimit());
        new Thread(this).start();
    }

    @Override
    public void stopDriverThread() {
        newSearch = false;
    }

    /**
     * This function calcules what is the minimum power a kuti need before going
     * to a power unit. It is dynamic and depends on the configuration of the
     * simulation parameters.
     * PRE : KSpeed != 0
     */
    private double calculatePowerLimit() {
        //calculate diagonal of the map
        double diag = Math.sqrt(Math.pow(ISimulParams.WORLD_HEIGHT, 2)
                + Math.pow(ISimulParams.WORLD_WIDTH, 2));
        //distance / speed = time (tiks)
        double energyTime = diag / isp.getKSpeed(false);
        // tiks * moving energy = total energy to travel distance.
        double energyTik = energyTime * isp.getKMovingEnergy();


        //find unit rotation energy
        double fURE = MAXROTATE * isp.getKRotationEnergy() * isp.getRadarEnergy();

        //little margin just in case.
        int marginEnergy = 50;

        return energyTik + fURE + marginEnergy;

    }

    // This function will get the Kuti closer to the power unit at each tik
    //PRE : --.
    private void gotoPowerUnit() {
        while (true) {
            double energy = iok.getEnergyLevel();
            move();
            if (ia.isColliding()) {
                manageCollision();
                changeOrientation(kutiMap.chooseDirection(newSearch));
                ia.waitNextStep();
            }

            //pour pas que ça plante: a changer?
            kutiMap.chooseDirection(newSearch);
            //arrondire au dizaines.
            double round = Math.round((kutiMap.getCrtPos().distance(kutiMap.getPowerUnitPos())) / 10) * 10;
            if (round == 0) {
                newSearch = true;
                waitOnPowerUnit();
                return;
            }
        }
    }

    /**
     * Find the distance to the powerUnit (use radar)
     *
     * @return a scanned distance to the powerUnit if no powerUnit is found
     * return the value -1
     */
    private double powerUnitDistance() {
        // ia.setMovingState(EKutiState.STOPPED);
        // ia.waitNextStep();
        ia.updateRadarData();
        ia.waitNextStep();
        Map<IObservable, Double> powerMap = iradar.getRadarElements();
        Iterator itr = powerMap.keySet().iterator();
        while (itr.hasNext()) {
            IObservable io = (IObservable) itr.next();
            if (io.getElementType() == EWorldElementType.CHARGING_STATION) {
                kutiMap.addPowerUnitPoint(powerMap.get(io));
                return powerMap.get(io);
            }
        }
        return -1;
    }

    /**
     * The kuti will wait on the power unit until he is entirely recharged
     */
    private void waitOnPowerUnit() {
        double orientation = Math.PI;
        ia.setMovingState(EKutiState.STOPPED);
        double energy = iok.getEnergyLevel();
        ia.waitNextStep();
        ia.waitNextStep();
        if (energy == iok.getEnergyLevel()) {

            double dist = powerUnitDistance();
            while (dist == -1) {
                changeOrientation(orientation);
                ia.waitNextStep();
                dist = powerUnitDistance();
                if (orientation > Math.PI / 6) {
                    orientation = orientation / 2;
                }
            }

            kutiMap.addPowerUnitPoint(dist);
            gotoPowerUnit();
            return;
        }
        //Stay on the power unit
        while (isp.getKMaxEnergy() != iok.getEnergyLevel()) {
            kutiMap.updatePowerUnitPos();
            ia.waitNextStep();
        }
    }

    // this function will be used when the kuti needs to find  a powerUnit to
    // charge its battery. return distance, -1 if not found
    // PRE : kutis STOPPED 
    private void findPowerUnit() {
        double phaseShift = Math.PI / 2; // used to calculate next rotation
        int oCounter = 0; // Orientation counter used to search power unit
        int max = MAXROTATE;
        double distance;
        while (!kutiMap.getIsPowerUnitFound() && max != 0) {
            max--;
            if (oCounter == 4 && phaseShift > MAXSCHIFT) {
                oCounter = 0;
                phaseShift = phaseShift / 2;
            }
            //update radar
            distance = powerUnitDistance();
            if (distance == -1) {
                changeOrientation(phaseShift);
                oCounter++;
            }

            ia.waitNextStep();
        }
        if (!kutiMap.getIsPowerUnitFound()) {
            powerUnitExists = false;
        }


    }

    /**
     * checks if energyLevel is still high enough or should be recharged.
     *
     * @return true : batteryOk, false : batteryNOK
     */
    private boolean isBatteryOk() {
        return (iok.getEnergyLevel() > minEnergy);
    }

    /**
     * kuti will be aligned with the wall! PRE: must touch a wall (end of world)
     * and arrive again the wall
     */
    private void alignToWall() {
        // look for the first point
        ia.setMovingState(EKutiState.STOPPED);
        ia.waitNextStep();
        Point2D p1 = new Point2D.Double(kutiMap.getCrtPos().getX(), kutiMap.getCrtPos().getY());
        kutiMap.getCrtPos();
        changeOrientation(Math.PI);
        ia.waitNextStep();
        move();
        // look for the second point
        ia.setMovingState(EKutiState.STOPPED);
        ia.waitNextStep();
        changeOrientation(-Math.PI / 2);
        Point2D p2 = new Point2D.Double(kutiMap.getCrtPos().getX(), kutiMap.getCrtPos().getY());
        ia.waitNextStep();
        advanceUntilCollision();
        // look for the third point
        ia.setMovingState(EKutiState.STOPPED);
        ia.waitNextStep();
        Point2D p3 = new Point2D.Double(kutiMap.getCrtPos().getX(), kutiMap.getCrtPos().getY());
        // calcul the distance beetween the 3 point to construct a triangle
        double da = p2.distance(p1);
        double db = p3.distance(p1);
        // calcul the angle for the rotation and be parallel to the wall
        double alpha = Math.asin(da / db);
        changeOrientation(-Math.PI + alpha);
        ia.waitNextStep();
        alined = true;
    }

    /**
     * After kuti's align to the wall and it touch a wall, it come back to cut
     * the lawn in a parallel strip
     */
    private void returnFoot() {
        double angle=Math.PI;
        if(cptTurn%2==0){
            angle=-Math.PI;
        }
        ia.setMovingState(EKutiState.STOPPED);
        ia.waitNextStep();
        changeOrientation((angle / 2));
        ia.waitNextStep();
        move();
//        if (ia.isColliding()) {
//            ia.setMovingState(EKutiState.STOPPED);
//            ia.waitNextStep();
//            changeOrientation(angle);
//            ia.waitNextStep();
//            move();
//        }
        ia.setMovingState(EKutiState.STOPPED);
        ia.waitNextStep();
        changeOrientation(angle / 2);
        ia.waitNextStep();
        move();
//        if (ia.isColliding()) {
//            ia.setMovingState(EKutiState.STOPPED);
//            ia.waitNextStep();
//            changeOrientation(angle);
//            ia.waitNextStep();
//        }
        cptTurn++;
    }

    /**
     * Analyse the radar's results to determine if the way is free until the end
     * //PRE : Kuti is stopped
     *
     * @return true if the way is free to the end of the world
     */
    private boolean isWayFree() {
        ia.setMovingState(EKutiState.STOPPED);
        ia.waitNextStep();
        ia.updateRadarData();
        ia.waitNextStep();
        Map<IObservable, Double> objMap = iradar.getRadarElements();
        Iterator<IObservable> itr = objMap.keySet().iterator();
        int cpt = 0;
        while (itr.hasNext()) {
            if (itr.next().getElementType() == EWorldElementType.OBSTACLE) {
                cpt++;
            }
        }
        return (cpt < 2);
    }

    /**
     * Kuti changes orientation and give new orientation to kutiMap PRE : Kuti
     * stopped
     */
    public void changeOrientation(double orientation) {
        orientation = PiMinusToPi(orientation);
        ia.changeOrientation(orientation);
        kutiMap.setOrientation(orientation);
    }

    /**
     * Kuti moves, waits a step and updates his relativ current position.
     */
    public void move() {
        ia.setMovingState(EKutiState.MOVING);
        isMoving = true;
        ia.waitNextStep();
        kutiMap.updateCrtPos(ia.traveledDistance());
    }

    private void stopMoving() {
        ia.setMovingState(EKutiState.STOPPED);
        isMoving = false;
    }

    /**
     * Kuti starts the cutter and updates variable
     */
    public void startCutting() {
        ia.setCutterState(ECutterState.ON);
        isCutting = true;
    }

    /**
     * Kuti stops the cutter and updates variable
     */
    public void stopCutting() {
        ia.setCutterState(ECutterState.OFF);
        isCutting = false;
    }

    /**
     * the kuti will choose a random direction Random going from -pi to pi
     *
     * @return
     */
    private double randomDirection() {
        Random r = new Random();
        return ((r.nextDouble() * (Math.PI + Math.PI)) - Math.PI);
    }

    /**
     * this will regulate values and only let them be between -PI and PI
     */
    public static double PiMinusToPi(double value) {
        // modulo of 2PI
        value = value % (2 * Math.PI);
        // add or remove 2PI depending on value.
        if (value > Math.PI) {
            value = value - (2 * Math.PI);
        } else if (value < -Math.PI) {
            value = value + (2 * Math.PI);
        }
        if (value > Math.PI || value < -Math.PI) {
            System.out.println(value);
        }
        return value;
    }

    @Override
    public void run() {



        algo2();
        // algo1();

//        int step = 0;
//        
//        while(true){ step ++;
//        
//        ia.updateRadarData(); ia.changeOrientation(Math.PI/180);
//        ia.waitNextStep();
//        
//        Iterator it = iradar.getRadarElements().keySet().iterator();
//        while(it.hasNext()){ IObservable key = (IObservable) it.next();
//        if(key.getElementType()==EWorldElementType.CHARGING_STATION){
//            gotoPowerUnit(findPowerUnit());
//        }
//        double d = iradar.getRadarElements().get(key);
//        
//        System.out.println(step + ") " + d + " - " + key);
//        
//        
//        }
//        
//        
//        }



        //algo2();
    }

    /**
     * Kuti will go around the obstacle PRE : kuti is in a collision
     */
    private void manageCollision() {
        double orientation = Math.PI / 2;
        double distance = Double.MAX_VALUE;
        int direction = -1;
        Map<IObservable, Double> objMap;
        Iterator<IObservable> itr;
        //tourner deux fois dans la boucle pour scanner a gauche et à droite
        for (int i = 0; i < 2; i++) {
            changeOrientation(orientation);
            ia.updateRadarData();
            ia.waitNextStep();
            objMap = iradar.getRadarElements();
            itr = objMap.keySet().iterator();
            while (itr.hasNext() && objMap.size() > 1) {
                IWorldElement elt = itr.next();
                if (elt.getElementType() == EWorldElementType.OBSTACLE) {
                    distance = Math.min(distance, objMap.get(elt));
                }
            }
            if (distance == Double.MAX_VALUE) {
                break;
            }
            orientation = -Math.PI;
            direction = 1;
        }

        int cnt = 0;
        for (int i = 0; i < 10; i++) {
            if (!ia.isColliding()) {
                cnt = 0;
                move();
            } else {
                cnt++;
                if (cnt != 0) {
                    changeOrientation(orientation);
                    ia.waitNextStep();
                }
            }
        }
        stopMoving();
        ia.waitNextStep();
        changeOrientation(Math.PI / 2 * direction);
        ia.waitNextStep();

        for (int i = 0; i < 10; i++) {
            if (!ia.isColliding()) {
                cnt = 0;
                move();
            } else {
                cnt++;
                if (cnt != 0) {
                    changeOrientation(orientation);
                    ia.waitNextStep();
                }
            }
        }
        stopMoving();
        ia.waitNextStep();
        /**
         * for (int i = 0; i < 5; i++) { while (ia.isColliding()) {
         * changeOrientation((Math.PI / 1.1)); ia.waitNextStep(); move();
         *
         * }
         * move(); } ia.setMovingState(EKutiState.STOPPED); ia.waitNextStep();
         */
    }

    /**
     * Kuti goes forward until collision.
     */
    private void advanceUntilCollision() {
        while (!ia.isColliding()) {
            if (!isBatteryOk()) {
                break;
            }
            move();
        }
    }

    public void waitForHelp() {
        while (!isBatteryOk()) {
            ia.waitNextStep();
        }
    }

    /**
     * this algorithme is specified in the coneception diagramms
     */
    private void algo2() {
        boolean first = true;
        while (true) {
            while (isBatteryOk()) {
                if (first) {
                    stopCutting();
                    while (!isWayFree()) {
                        //stopMoving();
                        changeOrientation(randomDirection());
                        ia.waitNextStep();
                        //get out if battery isn't ok
                        if (!isBatteryOk()) {
                            break;
                        }
                        //tobe continued
                    }
                    first = false;
                }
                startCutting();
                advanceUntilCollision();
                if (isMoving) {
                    stopMoving();
                    ia.waitNextStep();
                }

                //alignToWall();
                // TODO returnFoot appel
                if (isBatteryOk()) {
                    if (!alined) {
                        alignToWall();
                    } else {
                        returnFoot();
                    }
                } else {
                    stopMoving();
                    ia.waitNextStep();
                }
            }

            if (!isBatteryOk() && powerUnitExists) {
                stopCutting();
                double distance = -1;
                while (true) {
                    // if powerUnit not found

                    if (kutiMap.getIsPowerUnitFound()) {
                        changeOrientation(kutiMap.chooseDirection(newSearch));
                        ia.waitNextStep();
                        newSearch = false;
                    } else {
                        findPowerUnit();
                    }
                    if (kutiMap.getIsPowerUnitFound()) {
                        gotoPowerUnit();
                    } else {
                        waitForHelp();
                    }
                    if (isBatteryOk()) {
                        break;
                    }
                }
            }
        }
    }

    /**
     * this algorithme is specified in the conception diagrams.
     */
    private void algo1() {
        while (true) {
            // chercher borne au début, mais avant cela il faut que chooseDirection fonctionne.
            startCutting();
            if (scanThreeWays()) {
                advanceUntilCollision();
                changeOrientation(Math.PI / 2);
                ia.waitNextStep();
            } else {
                changeOrientation(Math.PI / 2);
                ia.waitNextStep();
            }
        }
    }

    /**
     * Scans on the left in front and on the right to minimize the chances of
     * hitting an obstacle while advancing. In specification, in a tik, can only
     * scan and turn once. PRE: STOPPED
     */
    private boolean scanThreeWays() {


        //turn left and scan
        changeOrientation(Math.PI / 8);
        if (!isWayFree()) {
            return false;
        }

        changeOrientation(-Math.PI / 4);
        if (!isWayFree()) {
            return false;
        }

        //scan in front
        changeOrientation(Math.PI / 8);
        if (!isWayFree()) {
            return false;
        }

        return true;
    }
}
